#include "Navigation.h"

namespace uav
{
namespace nav
{

void Navigation::initialize(const RefFrame &ref, const Vector3 &p, const Vector3 &v, const Quaternion &q)
{
	refFrame = ref;
	position = p;
	velocity = v;
	orientation = q;
	gyroBias = Vector3::Zero;
	accelBias = Vector3::Zero;
}

void Navigation::update(const Vector3 &w, const Vector3 &a, const Fixed &dt)
{
	position += velocity * dt;

	// Correct IMU data using estimated biases
	Vector3 wc = w, ac = a;
	wc -= gyroBias;
	ac -= accelBias;

	// Correct acceleration for IMU offset from CG
	Vector3 temp1 = wc.cross(imuOffset);
	Vector3 temp2 = wc.cross(temp1);
	temp1 = ac - temp2;
	// Transform acceleration from body frame to ENU frame and adjust for gravity
	ac = orientation.rotateInverse(temp1);
	ac.z -= NAV_G;

	velocity += ac * dt;

	// Compute quaternion derivative for orientation update: q(t + dt) = exp(0.5 * (0, w) * dt) * q(t)
	wc *= dt;
	wc.x >>= 1; wc.y >>= 1; wc.z >>= 1;
	Fixed s = wc.length(), sinS, cosS;
	s.sinCos(sinS, cosS);
	wc /= s;
	Quaternion derivative(cosS, wc * sinS);

	orientation *= derivative;
}

void Navigation::correct(const GeoCoord &geo, const Vector3 &v)
{

}

}} // end namespace
